
// Dynamixel SDK platform dependent source
#include "dxl_hal.h"
#include "stm32fwlib.h"

static USART_TypeDef used_usart; //FIXME default value (or counter so we can't close before initialized

int dxl_hal_open( USART_TypeDef* USARTx, u32 baudrate )
{
	// Opening device
	// devIndex: Device index
	// baudrate: Real baudrate (ex> 115200, 57600, 38400...)
	// Return: 0(Failed), 1(Succeed)
	
	USART_InitTypeDef params;
	params.USART_BaudRate = baudrate;
	params.USART_WordLength = USART_WordLength_8b;
	params.USART_StopBits = USART_StopBits_1;
	params.USART_Parity = USART_Parity_No;
	params.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	params.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; //FIXME should we specify both?
	
	USART_Init(USARTx, &params);
	
	used_usart = *USARTx;
	
	return 1;
}

void dxl_hal_close()
{
	// Closing device
	USART_DeInit(&used_usart);
}

void dxl_hal_clear(void)
{
	// Clear communication buffer

}

int dxl_hal_tx( unsigned char *pPacket, int numPacket )
{
	// Transmiting date
	// *pPacket: data array pointer
	// numPacket: number of data array
	// Return: number of data transmitted. -1 is error.

	for (int n = 0; n < numPacket; n++)
	{
		//FIXME Do we need this:
		//FlagStatus status = USART_GetFlagStatus(used_usart, USART_FLAG_TXE); // check Transmit data register empty flag
		USART_SendData(&used_usart, pPacket[n]);
	}
	return numPacket;
}

int dxl_hal_rx( unsigned char *pPacket, int numPacket )
{
	// Recieving date
	// *pPacket: data array pointer
	// numPacket: number of data array
	// Return: number of data recieved. -1 is error.
	for (int n = 0; n < numPacket; n++)
	{
		pPacket[n] = (unsigned char) USART_ReceiveData(&used_usart);
	}
	return numPacket;
}

void dxl_hal_set_timeout( int NumRcvByte )
{
	// Start stop watch
	// NumRcvByte: number of recieving data(to calculate maximum waiting time)

//FIXME allocate some timer...
}

int dxl_hal_timeout(void)
{
	// Check timeout
	// Return: 0 is false, 1 is true(timeout occurred)


//FIXME chek some timer...
}
#if 0
int dxl_hal_tx( unsigned char *pPacket, int numPacket )
{
	// Transmiting date
	// *pPacket: data array pointer
	// numPacket: number of data array
	// Return: number of data transmitted. -1 is error.
	int rc = write(dxl_fd, pPacket, numPacket);

	timespec ts;
	ts.tv_sec=0;
	ts.tv_nsec=1000000; // 1ms
	nanosleep(&ts,0);

	return rc;
}

int dxl_hal_rx( unsigned char *pPacket, int numPacket )
{
	// Recieving date
	// *pPacket: data array pointer
	// numPacket: number of data array
	// Return: number of data recieved. -1 is error.
	int rc = read(dxl_fd, pPacket, numPacket);
	return rc;
}

void dxl_hal_set_timeout( int NumRcvByte )
{
	// Start stop watch
	// NumRcvByte: number of recieving data(to calculate maximum waiting time)
	timeval tv;
	gettimeofday(&tv, NULL);
	long usecs_to_timeout = (1+NumRcvByte)*10.0/dxl_baudrate + tv.tv_usec;

	tv.tv_usec += usecs_to_timeout;

	while(tv.tv_usec > 1000000)
	{
		tv.tv_sec ++;
		tv.tv_usec -= 1000000;
	}
	dxl_timeout_tv = tv;
}

int dxl_hal_timeout(void)
{
	// Check timeout
	// Return: 0 is false, 1 is true(timeout occurred)

	timeval tv;
	gettimeofday(&tv, NULL);

	if( (tv.tv_sec > dxl_timeout_tv.tv_sec) || ((tv.tv_sec == dxl_timeout_tv.tv_sec) && (tv.tv_usec > dxl_timeout_tv.tv_usec) ))
		return 1;
	else
		return 0;
}
#endif
